//
// Created by xujingyi on 2021/5/16.
//

#include "solve/solve_PNP.h"
#include "Setter.h"
#include "find/armor_class.h"
#include "systime.h"
#include "log.h"

using namespace cv;
extern rm::Setter rm_setter;

rm::SolvePNP::SolvePNP() {
    _cameraMatrix = rm_setter.camera_matrix;
    _distortionCoefficients = rm_setter.distcoeffs;
}

//补偿因为剪切图片造成的图像偏移问题(if calib with unextracted picture)
std::vector<Point2f> cameraExtractCompensate(std::vector<Point2f> box_point2fs,cv::Point2f coef){
    for(std::vector<Point2f>::iterator it=box_point2fs.begin();it!=box_point2fs.end();++it){
        *it+=coef;
    }
    return box_point2fs;
}

void anticameraExtractCompensate(std::vector<Point2f> &box_point2fs,cv::Point2f coef){
    for(std::vector<Point2f>::iterator it=box_point2fs.begin();it!=box_point2fs.end();++it){
        *it-=coef;
    }
}

void rm::SolvePNP::drawCSYS(Mat img) const
{
    std::vector<Point2f> tuxiang;
    std::vector<Point3f> mubiao;
    mubiao.push_back(Point3f(0, 0, 0));
    mubiao.push_back(Point3f(100, 0, 0));
    mubiao.push_back(Point3f(0, 100, 0));
    mubiao.push_back(Point3f(0, 0, 100));

    projectPoints(mubiao, _rvecs, _tvecs, _cameraMatrix, _distortionCoefficients, tuxiang);//（世界坐标系坐标系到处图像坐标系）根据所给3D坐标和已知d几何变换求解投影后的2D坐标
    cameraExtractCompensate(tuxiang,_coef);

    anticameraExtractCompensate(tuxiang,_coef);
    circle(img, tuxiang[0], 3, Scalar(0, 0, 255), 1, LINE_8, 0);
    //绘制所取世界坐标系的x、y、z轴
    line(img, tuxiang[0], tuxiang[1], Scalar(0, 0, 255));//red:axis x
    line(img, tuxiang[0], tuxiang[2], Scalar(0, 255, 0));//green:axis y
    line(img, tuxiang[0], tuxiang[3], Scalar(128, 255, 255));//yellow:axis z
}

//extern cv::Mat testboard;
extern cv::Mat show_pic;
cv::Vec3f rotationMatrixToEulerAngles(cv::Mat &R);
bool rm::SolvePNP::operator()(rm::MarkerCouple &couple, const cv::Point2f offset, cv::Mat &img2show)
{
    systime t1,t2;
    getsystime(t1);
    clear();
    float refer_width,refer_height;
    if (couple.boxsize() == rm::BIG_BOX ) {
        refer_width = BIG_ARMOR_BOX_W;
        refer_height = BIG_ARMOR_BOX_H;

    }
    else if(couple.boxsize() == rm::SMALL_BOX ){
        refer_width = SMALL_ARMOR_BOX_W;
        refer_height = SMALL_ARMOR_BOX_H;
    }
    else {
        LOGE("PNP used by a NOT_BOX!");
        return false;
    }

    //左下、左上、右上、右下
    _refer_point3fs.push_back(Point3f(-refer_width / 2, -refer_height / 2, 0));//左下
    _refer_point3fs.push_back(Point3f(-refer_width / 2, refer_height / 2, 0));//左上
    _refer_point3fs.push_back(Point3f(refer_width / 2, refer_height / 2, 0));//右上
    _refer_point3fs.push_back(Point3f(refer_width / 2, -refer_height / 2, 0));//右下

//    cv::rectangle(testboard,boundingRect(armor_box.couple.vertexes()),cv::Scalar(155,0,0),2);
    if(couple.vertexes().size() == 4) _PNP_box_point2fs=cameraExtractCompensate(couple.vertexes(),_coef);//_coef
    else {LOGE("PNP_vertexes num is not 4!");return false;}
//    cout<<"distortionCoefficients"<<endl<<distortionCoefficients<<endl;
    solvePnP(_refer_point3fs, _PNP_box_point2fs, _cameraMatrix, _distortionCoefficients, _rvecs, _tvecs, false, SOLVEPNP_ITERATIVE);
    //从世界坐标系（接收目标世界3D坐标，一般取3D平面z坐标为0）到相机坐标系，得到rves（旋转），revs（平移）
    //    cout<<rvecs.size<<endl;
    //cv::Mat Rvec;
    //rvecs.convertTo(Rvec, CV_32F);    //旋转向量
    //cv::Mat_<float> Tvec;
    //taux.convertTo(Tvec, CV_32F);   //平移向量

    cv::Mat_<TYPE_POS> rotMat(3, 3);
    cv::Rodrigues(_rvecs, rotMat);  //由于SolvePNP返回的是旋转向量，故用罗德里格斯变换变成旋转矩阵
    cv::Vec3f anglevec=rotationMatrixToEulerAngles(rotMat);
    if(anglevec[0]>0) {
        anglevec[0]=-anglevec[0];
        anglevec[1]=-anglevec[1];
    }

//    指针类型注意要和tvec的定义对应！
    couple.setPNPCenter(cv::Point3f(_tvecs.ptr<TYPE_POS>(0)[0],_tvecs.ptr<TYPE_POS>(0)[1], abs(_tvecs.ptr<TYPE_POS>(0)[2])),anglevec);
//    std::cout<<abs(_tvecs.ptr<TYPE_POS>(0)[2])<<std::endl;
//    couple.setPNPCenter(cv::Point3f(_tvecs.ptr<TYPE_POS>(0)[0],_tvecs.ptr<TYPE_POS>(0)[1], 0.8493*abs(_tvecs.ptr<TYPE_POS>(0)[2])+265.5),anglevec);
//    armor_box.box_pos.setPNPCenter(armor_box.center(),cv::Point3f(_tvecs.ptr<TYPE_POS>(0)[0],_tvecs.ptr<TYPE_POS>(0)[1], 0.894*abs(_tvecs.ptr<TYPE_POS>(0)[2])+265.5),anglevec);
//    std::cout<<"pnp:"<<cv::Point3f(_tvecs.ptr<TYPE_POS>(0)[0],_tvecs.ptr<TYPE_POS>(0)[1],_tvecs.ptr<TYPE_POS>(0)[2])<<std::endl;
    //    TransformCamera2PTZ(armor_box);

//    	uchar *ptrbegin = tvecs.data;
//	//平移矩阵表示世界坐标系到相机坐标系中心需要平移的距离
//	const uchar *ptrend = tvecs.data + 3;
//	//    for(; ptrbegin != ptrend; ptrbegin++){
//	//        cout<<(int)(*ptrbegin)<<endl;
//	//    }
//	armor_box.box_pos.x = *(ptrend - 3);
//	armor_box.box_pos.y = *(ptrend - 2);
//	armor_box.box_pos.z = *(ptrend - 1);
//	cout<<"armor_box.box_pos.x:"<<armor_box.box_pos.x<<endl;

//	cout << "pnp_pitch:" << armor_box.box_pos.pitch << "  " << "pnp_yaw:" << armor_box.box_pos.yaw << endl;

#ifdef SHOW_PNP_BOX
    line(img2show, PNP_box_point2fs[0], PNP_box_point2fs[1], CV_RGB(0, 0, 255), 2, 8, 0);
	line(img2show, PNP_box_point2fs[1], PNP_box_point2fs[2], CV_RGB(0, 0, 255), 2, 8, 0);
	line(img2show, PNP_box_point2fs[2], PNP_box_point2fs[3], CV_RGB(0, 0, 255), 2, 8, 0);
	line(img2show, PNP_box_point2fs[3], PNP_box_point2fs[0], CV_RGB(0, 0, 255), 2, 8, 0);
#endif // SHOW_PNP_BOX

#ifdef BOX_POS_INFO
    showTxt(armor_box);
#endif // BOX_POS_INFO
    getsystime(t2);
    LOGT("solvePNP:%lf",getTimeIntervalms(t2,t1));
    return true;
}

void rm::SolvePNP::clear(){
    _refer_point3fs.clear();
    _PNP_box_point2fs.clear();
}

cv::Mat eulerAnglesToRotationMatrix(cv::Vec3f &theta)
{
    // Calculate rotation about x axis
    cv::Mat R_x = (cv::Mat_<double>(3,3) <<
                                         1,       0,              0,
            0,       cos(theta[0]),   -sin(theta[0]),
            0,       sin(theta[0]),   cos(theta[0])
    );
    // Calculate rotation about y axis
    cv::Mat R_y = (cv::Mat_<double>(3,3) <<
                                         cos(theta[1]),    0,      sin(theta[1]),
            0,               1,      0,
            -sin(theta[1]),   0,      cos(theta[1])
    );
    // Calculate rotation about z axis
    cv::Mat R_z = (cv::Mat_<double>(3,3) <<
                                         cos(theta[2]),    -sin(theta[2]),      0,
            sin(theta[2]),    cos(theta[2]),       0,
            0,               0,                  1
    );
    // Combined rotation matrix
    cv::Mat R = R_z * R_y * R_x;
    return R;
}

cv::Vec3f rotationMatrixToEulerAngles(cv::Mat &R)
{
    float sy = sqrt(R.at<TYPE_POS>(0,0) * R.at<TYPE_POS>(0,0) +  R.at<TYPE_POS>(1,0) * R.at<TYPE_POS>(1,0) );
    bool singular = sy < 1e-6; // If
    float x, y, z;
    if (!singular)
    {
        x = atan2(R.at<TYPE_POS>(2,1) , R.at<TYPE_POS>(2,2));
        y = atan2(-R.at<TYPE_POS>(2,0), sy);
        z = atan2(R.at<TYPE_POS>(1,0), R.at<TYPE_POS>(0,0));
    }
    else
    {
        x = atan2(-R.at<TYPE_POS>(1,2), R.at<TYPE_POS>(1,1));
        y = atan2(-R.at<TYPE_POS>(2,0), sy);
        z = 0;
    }
#if 1
    x = x*180.0f/3.141592653589793f;
    y = y*180.0f/3.141592653589793f;
    z = z*180.0f/3.141592653589793f;
#endif
    return cv::Vec3f(x, y, z);
}